// Copyright (c) 2022 ChenJun
// Licensed under the Apache-2.0 License.

#pragma once

#include <tf2_ros/transform_broadcaster.h>

#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/subscription.hpp>
// #include <serial_driver/serial_driver.hpp>
#include <std_msgs/msg/float64.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <visualization_msgs/msg/marker.hpp>

#include "tf2_ros/buffer.h"

#include "tf2_ros/transform_listener.h"
// C++ system
#include <future>
#include <memory>
#include <string>
#include <thread>
#include <vector>
#include <mutex>

#include "auto_aim_interfaces/msg/target.hpp"

#include "auto_aim_interfaces/msg/dangle_shoot.hpp"
#include "std_msgs/msg/u_int16.hpp"
#include "auto_aim_interfaces/msg/cmd_up_yaw_volt_shoot.hpp"


#define CANUSB_INJECT_SLEEP_GAP_DEFAULT 200 /* ms */
#define CANUSB_TTY_BAUD_RATE_DEFAULT 2000000

namespace anti_spinner
{
class AntiSpinner : public rclcpp::Node
{
public:

  explicit AntiSpinner(const rclcpp::NodeOptions & options);

  ~AntiSpinner() override;

private:
  void getParams();

  void setParam(const rclcpp::Parameter & param);

  void resetTracker();

  // void 
  void predict_and_pub_ctr(const auto_aim_interfaces::msg::Target::SharedPtr &msg, float pipeline_process_latency_ms);
  void decideFollow(auto_aim_interfaces::msg::Target::SharedPtr msg);

  // compensation coefficients
  float earth_comp_k_;
  float earth_comp_b_;
  float static_error_electric_mechanics_pitch_;
  float static_error_electric_mechanics_yaw_;
  float shoot_action_delay_;
  float shoot_speed_;
  float critical_omega_;
  float shoot_gap_;

  // Param client to set detect_colr
  using ResultFuturePtr = std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>;
  bool initial_set_param_ = false;
  uint8_t previous_receive_color_ = 0;
  rclcpp::AsyncParametersClient::SharedPtr detector_param_client_;
  ResultFuturePtr set_param_future_;


  std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;

  // Service client to reset tracker
  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr reset_tracker_client_;

  // Aimimg point receiving from serial port for visualization
  visualization_msgs::msg::Marker aiming_point_;

  // Broadcast tf from odom to gimbal_link
  double timestamp_offset_ = 0;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

  rclcpp::Subscription<auto_aim_interfaces::msg::Target>::SharedPtr target_sub_;

  rclcpp::Publisher<auto_aim_interfaces::msg::DangleShoot>::SharedPtr float_ctr_publisher_;

  // For debug usage
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr latency_pub_;
  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_pub_;

};
}  // namespace anti_spinner
